#include <stdlib.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
	if((argc != 5) && (argc != 3))
	{
		ROS_INFO("rosrun cmd turtle_vel /<name>/cmd_vel <HZ> x z");
		ROS_INFO("rosrun cmd turtle_vel /<name>/cmd_vel 0");
		return -1;
	}
    // ROS节点初始化
    ros::init(argc, argv, "vel_publisher");

    // 创建节点句柄
    ros::NodeHandle n;

    ros::Publisher person_info_pub = n.advertise<geometry_msgs::Twist>(argv[1], 20);

    // 设置循环的频率
    ros::Rate loop_rate(atoi(argv[2]));

    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_communication::Person类型的消息
		geometry_msgs::Twist vel_msg;

		if(argc == 2)
		{
			vel_msg.linear.x = 0;	
			vel_msg.angular.z = 0; 
		}	
		else if (argc == 5)
		{
			vel_msg.linear.x = atof(argv[3]);
			vel_msg.angular.z = atof(argv[4]); 
		}
        // 发布消息
		person_info_pub.publish(vel_msg);

//		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]",
                          //      vel_msg.linear.x, vel_msg.angular.z);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}
